#include "BPEVector3d.h"

#include <math.h>
#include "BPEMatrix.h"

BPEVector3d::BPEVector3d(){ 
	tail = BPEPoint3d();
	head = BPEPoint3d();
};	

BPEVector3d::BPEVector3d(const double &x,const double &y,const double &z){
	tail = BPEPoint3d();
	head = BPEPoint3d(x,y,z);
};

BPEVector3d::BPEVector3d(const BPEPoint3d &tail,const BPEPoint3d &head){
	this->tail = tail;
	this->head = head;
};

BPEVector3d::BPEVector3d(const BPEPoint3d &tail, 
						 const double &radsXY, const double &radsXZ 
						 ,const double &magnitude){

	this->tail = tail;
	this->head = this->tail + BPEPoint3d(magnitude,0,0);


	this->rotateXY(radsXY);
	this->rotateXZ(radsXZ);

};


BPEPoint3d BPEVector3d::getHead(){
	return this->head;
};
BPEPoint3d BPEVector3d::getTail(){
	return this->tail;	
};

void BPEVector3d::setHead(const BPEPoint3d &h){
	this->head = h;
};
void BPEVector3d::setTail(const BPEPoint3d &t){
	this->tail = t;
};

double BPEVector3d::getXYRads(){
	double rads;
	if(head.getX()-tail.getX() > 0 && head.getY()-tail.getY() != 0)
		rads = atan((double)(head.getY() - tail.getY() )/(double)(head.getX() -tail.getX()));
	else if(head.getX()-tail.getX() < 0 && head.getY()-tail.getY() != 0)
		rads = atan((double)(head.getY()-tail.getY())/(double)(head.getX()-tail.getX())) - pi;

	//0,	pi/2,	2*pi,	3*pi/2
	else if(head.getX()-tail.getX() ==0 && head.getY() > tail.getY())
		rads = pi/2;
	else if(head.getX()-tail.getX() ==0 && head.getY() < tail.getY())
		rads = -pi/2;
	else if(head.getY()-tail.getY() ==0 && head.getX()>tail.getX())
		rads = 0;
	else if(head.getY()-tail.getY() ==0 && head.getX()<tail.getX())
		rads = pi;
	else
		rads = 0;

	double _2pi = 2*pi;
	if(rads < 0) rads += _2pi;
	else if(rads > _2pi) rads-=_2pi;	

	return rads;
	
};
double BPEVector3d::getZYRads(){
	double rads;
	if(head.getZ()-tail.getZ() > 0 && head.getY()-tail.getY() != 0)
		rads = atan((double)(head.getY() - tail.getY() )/(double)(head.getZ() -tail.getZ()));
	else if(head.getZ()-tail.getZ() < 0 && head.getY()-tail.getY() != 0)
		rads = atan((double)(head.getY()-tail.getY())/(double)(head.getZ()-tail.getZ())) - pi;

	//0,	pi/2,	2*pi,	3*pi/2
	else if(head.getZ()-tail.getZ() ==0 && head.getY() > tail.getY())
		rads = pi/2;
	else if(head.getZ()-tail.getZ() ==0 && head.getY() < tail.getY())
		rads = -pi/2;
	else if(head.getY()-tail.getY() ==0 && head.getZ()>tail.getZ())
		rads = 0;
	else if(head.getY()-tail.getY() ==0 && head.getZ()<tail.getZ())
		rads = pi;
	else
		rads = 0;
	
	double _2pi = 2*pi;
	if(rads < 0) rads += _2pi;
	else if(rads > _2pi) rads-=_2pi;

	return rads;
};
double BPEVector3d::getXZRads(){

	double rads;
	if(head.getX()-tail.getX() > 0 && head.getZ()-tail.getZ() != 0)
		rads = atan((double)(head.getZ() - tail.getZ() )/(double)(head.getX() -tail.getX()));
	else if(head.getX()-tail.getX() < 0 && head.getZ()-tail.getZ() != 0)
		rads = atan((double)(head.getZ()-tail.getZ())/(double)(head.getX()-tail.getX())) - pi;

	//0,	pi/2,	2*pi,	3*pi/2
	else if(head.getX()-tail.getX() ==0 && head.getZ() > tail.getZ())
		rads = pi/2;
	else if(head.getX()-tail.getX() ==0 && head.getZ() < tail.getZ())
		rads = -pi/2;
	else if(head.getZ()-tail.getZ() ==0 && head.getX()>tail.getX())
		rads = 0;
	else if(head.getZ()-tail.getZ() ==0 && head.getX()<tail.getX())
		rads = pi;
	else
		rads = 0;


	double _2pi = 2*pi;
	if(rads < 0) rads += _2pi;
	else if(rads > _2pi) rads-=_2pi;

	return rads;

};


double BPEVector3d::getMagnitude(){
	//pythagorean theorem just that :D
	double magnitude;
	double xMag = pow(head.getX() -tail.getX() , 2);
	double yMag = pow(head.getY() -tail.getY() , 2);
	double zMag = pow(head.getZ() -tail.getZ() , 2);

	magnitude = sqrt(xMag+yMag+zMag);

	return magnitude;
};

void BPEVector3d::moveTo(const BPEPoint3d &t){
	//we calculate the difference between the old and the new tail
	double xDifference;
	double yDifference;
	double zDifference;

	BPEPoint3d temp = t;

	xDifference = temp.getX() - tail.getX();
	yDifference = temp.getY() - tail.getY();
	zDifference = temp.getZ() - tail.getZ();

BPEPoint3d difference(xDifference,yDifference,zDifference);

	tail = tail + difference;

	//we add this difference to the head
	head = head + difference;
};

BPEVector3d BPEVector3d::add(BPEVector3d v){
	BPEPoint3d oldHead = this->getHead();
	BPEPoint3d vHead = v.getHead();
	BPEPoint3d newHead = BPEPoint3d(oldHead + vHead);
		
	this->head = newHead;

	return BPEVector3d(*this);
};

double BPEVector3d::dotProduct(BPEVector3d other){
	
	/*//we create clones of our vectors because we will
	//change them in some ways to calculate the dot product
	BPEVector3d temp1 = BPEVector3d(*this);
	BPEVector3d temp2 = other;
	
	//we move the second vector to the first so they can share the same tail
	temp2.moveTo(temp1.getTail());


	//we want to handle them on the X0Y plane
	//so we create a vector which connects the heads of the two vectors
	//then we learn how much "angle" it has(from the Y0X plane)
	//at last we rotate both vectors the angle we found earlier...

	BPEVector3d connector(temp1.getHead(), temp2.getHead());
	
	double rotateAngle = -(connector.getZYRads() - (pi/2));

//	temp1.rotateZY(rotateAngle);
//	temp2.rotateZY(rotateAngle);


	//now both vectors share the same tail and are on X0Y plane

	double result=0;
	if(temp1.getXYRads() >= temp2.getXYRads())
		result = temp1.getMagnitude() * temp2.getMagnitude() * cos(temp1.getXYRads()-temp2.getXYRads());
	else 
		result = temp1.getMagnitude() * temp2.getMagnitude() * cos(temp2.getXYRads()-temp1.getXYRads());
		
	return result;
	*/

	double newX = this->getHead().getX() * other.getHead().getX();
	double newY = this->getHead().getY() * other.getHead().getY();
	double newZ = this->getHead().getZ() * other.getHead().getZ();
	return newX + newY + newZ;

}




BPEVector3d BPEVector3d::crossProduct(const BPEVector3d &other){

	//we create clones of our vectors because we will
	//change them in some ways to calculate the cross product
	BPEVector3d temp1 = BPEVector3d(*this);
	BPEVector3d temp2 = other;

	//we move them to the point(0,0,0)
	temp1.moveTo(BPEPoint3d(0,0,0));
	temp2.moveTo(BPEPoint3d(0,0,0));


	BPEPoint3d tHead1 = temp1.getHead();
	BPEPoint3d tHead2 = temp2.getHead();

	double resultX = tHead1.getY() * tHead2.getZ() - tHead1.getZ() * tHead2.getY();
	double resultY = tHead1.getZ() * tHead2.getX() - tHead1.getX() * tHead2.getZ();
	double resultZ = tHead1.getX() * tHead2.getY() - tHead1.getY() * tHead2.getX();

	BPEVector3d result (resultX,resultY,resultZ);
	result.moveTo(this->getTail());

	return result;

}
double BPEVector3d::findRadsDifference(BPEVector3d other){

	double dotProduct = this->dotProduct(other);
	double magnitudeThis = this->getMagnitude();
	double magnitudeOther = other.getMagnitude();

	double cosTheta = dotProduct / (magnitudeThis * magnitudeOther);
	double cosRads = cosTheta * pi /180;
	return acos(cosRads);

};
BPEVector3d BPEVector3d::getReflectionAgaintNormal(BPEVector3d normal){
	normal.normalize();
	BPEVector3d reflection;
	BPEVector3d projection = normal*(this->dotProduct(normal));
	BPEVector3d temp = BPEVector3d(projection.getHead(),this->getHead());
	temp.inverse();
	reflection = BPEVector3d(this->getTail(),temp.getHead());
	//BPEVector3d reflection = this->operator -(normal*(2*(this->dotProduct(normal)) ));
	return reflection;

};

double BPEVector3d::distanceFromAPoint(const BPEPoint3d &p){

	//we will use the cross product formula
	BPEVector3d clone(*this);

	clone.moveTo(BPEPoint3d(0,0,0));

	//now we are set to start the calculations
	//first we create a vector "tempVec" between the point p and a point on a the vector...
	///lets call the point of the vector x and let it be its head
	BPEPoint3d x = clone.getHead();
	
	BPEVector3d tempVec(p,x);


	//now we must find the cross product of tempVect and our vector
	BPEVector3d crossP = clone.crossProduct(tempVec);

	//now we need just the magnitude of that vector and we divide it by the magnitude of our vector and we are done
	
	return crossP.getMagnitude() / this->getMagnitude();
	
};

void BPEVector3d::rotateXY(double rads){

	/*
	if(rads >= 2*pi)
		rads -= 2*pi;
	if(rads <0)
		rads += 2*pi;


	double _cos;
	double _sin;

	if( rads == pi/2 || rads == 3*pi/2)
		_cos = 0;
	else
		_cos = cos(rads);

	if( rads == 0 || rads == pi)
		_sin = 0;
	else 
		_sin = sin(rads);




	BPEPoint3d originalPos = this->getTail();
	this->moveTo(BPEPoint3d(0,0,0));

	BPEPoint3d originalHead = this->getHead();
	double newHeadX = originalHead.getX()*_cos + originalHead.getY()*_sin;
	double newHeadY = -originalHead.getX()*_sin + originalHead.getY()*_cos;
	
	this->setHead(BPEPoint3d(newHeadX,newHeadY,this->getHead().getZ()));
	this->moveTo(originalPos);

	*/
	BPEMatrix xy(1,2);
	BPEMatrix rotationMatrix(2,2);

	BPEPoint3d tempTail = this->getTail();

	this->moveTo(BPEPoint3d(0,0,0));

	//fill the matrix of the xy coordinates
	xy.insertAt(head.getX(),0,0);
	xy.insertAt(head.getY(),0,1);

	//fill the rotation matrix

	if(rads >= 2*pi)
		rads -= 2*pi;
	if(rads <0)
		rads += 2*pi;


	double _cos;
	double _sin;

	if( rads == pi/2 || rads == 3*pi/2)
		_cos = 0;
	else
		_cos = cos(rads);

	if( rads == 0 || rads == pi)
		_sin = 0;
	else 
		_sin = sin(rads);



	rotationMatrix.insertAt(_cos,0,0);
	rotationMatrix.insertAt(_sin,0,1);
	rotationMatrix.insertAt(-_sin,1,0);
	rotationMatrix.insertAt(_cos,1,1);

	BPEMatrix newCoord = xy.multiply(rotationMatrix);

	double g = newCoord.getFrom(0,0);
	double f =	newCoord.getFrom(0,1);
	this->head.setX(newCoord.getFrom(0,0));
	this->head.setY(newCoord.getFrom(0,1));

	this->moveTo(tempTail);

};
void BPEVector3d::rotateZY(double rads){

	BPEMatrix zy(1,2);
	BPEMatrix rotationMatrix(2,2);

	BPEPoint3d tempTail = this->getTail();

	this->moveTo(BPEPoint3d(0,0,0));

	//fill the matrix of the xy coordinates
	zy.insertAt(head.getZ(),0,0);
	zy.insertAt(head.getY(),0,1);


	double _cos;
	double _sin;

	if( rads == pi/2 || rads == 3*pi/2)
		_cos = 0;
	else
		_cos = cos(rads);

	if( rads == 0 || rads == pi)
		_sin = 0;
	else 
		_sin = sin(rads);


	//fill the rotation matrix
	rotationMatrix.insertAt(_cos,0,0);
	rotationMatrix.insertAt(_sin,0,1);
	rotationMatrix.insertAt(-_sin,1,0);
	rotationMatrix.insertAt(_cos,1,1);

	BPEMatrix newCoord = zy.multiply(rotationMatrix);
	this->head.setZ(newCoord.getFrom(0,0));
	this->head.setY(newCoord.getFrom(0,1));

	this->moveTo(tempTail);

};
void BPEVector3d::rotateXZ(double rads){

	BPEMatrix xz(1,2);
	BPEMatrix rotationMatrix(2,2);

	BPEPoint3d tempTail = this->getTail();

	this->moveTo(BPEPoint3d(0,0,0));

	//fill the matrix of the xy coordinates
	xz.insertAt(head.getX(),0,0);
	xz.insertAt(head.getZ(),0,1);



	double _cos;
	double _sin;

	if( rads == pi/2 || rads == 3*pi/2)
		_cos = 0;
	else
		_cos = cos(rads);

	if( rads == 0 || rads == pi)
		_sin = 0;
	else 
		_sin = sin(rads);


	//fill the rotation matrix
	rotationMatrix.insertAt(_cos,0,0);
	rotationMatrix.insertAt(_sin,0,1);
	rotationMatrix.insertAt(-_sin,1,0);
	rotationMatrix.insertAt(_cos,1,1);

	BPEMatrix newCoord = xz.multiply(rotationMatrix);
	this->head.setX(newCoord.getFrom(0,0));
	this->head.setZ(newCoord.getFrom(0,1));

	this->moveTo(tempTail);

};


void BPEVector3d::inverse(){
	BPEPoint3d temp(this->getTail());
	this->setTail(this->getHead());
	this->setHead(temp);
	this->moveTo(temp);
	
};

void BPEVector3d::normalize(){
	this->moveTo(BPEPoint3d(0,0,0));
	BPEPoint3d oldHead = this->getHead();
	oldHead.setX( oldHead.getX() /this->getMagnitude());
	oldHead.setY( oldHead.getY() /this->getMagnitude());
	oldHead.setZ( oldHead.getZ() /this->getMagnitude());

	this->setHead(oldHead);

};
void BPEVector3d::setMagnitude(double mag){
	this->normalize();
	this->setHead(this->getHead() * mag);
};

BPEVector3d BPEVector3d::operator*(double scalar){
	BPEVector3d temp = BPEVector3d();
	temp.setHead(this->getHead()*scalar);


	return temp;
	
};
BPEVector3d BPEVector3d::operator/(double scalar){
	BPEVector3d temp = BPEVector3d();
	temp.setHead(this->getHead()/scalar);


	return temp;
	
};

BPEVector3d BPEVector3d::operator-(BPEVector3d other){

	BPEVector3d result = BPEVector3d(*this);
	result.setTail((result.getTail() - other.getTail()));
	result.setHead((result.getHead() - other.getHead()));
	return result;
	
};